//
//  OGLmap.m
//  CommandCenter08
//
//  Created by Matt Roman on 7/3/08.
//  Copyright 2008 University of Oklahoma. All rights reserved.
//

#import "OGLmap.h"
#import "NSFont_OpenGL.h"

#define FARCLIPPING		5001.0
#define NEARCLIPPING	0.5


@implementation OGLmap

// pixel format definition
+ (NSOpenGLPixelFormat*) basicPixelFormat
{
    NSOpenGLPixelFormatAttribute attributes [] = {
        NSOpenGLPFAWindow,
        NSOpenGLPFADoubleBuffer,	// double buffered
        NSOpenGLPFADepthSize, (NSOpenGLPixelFormatAttribute)16, // 16 bit depth buffer
        (NSOpenGLPixelFormatAttribute)nil
    };
    return [[[NSOpenGLPixelFormat alloc] initWithAttributes:attributes] autorelease];
}

-(id) initWithFrame: (NSRect) frameRect andRover:(Rover*)robot
{
	NSOpenGLPixelFormat * pf = [OGLmap basicPixelFormat];
	self = [super initWithFrame: frameRect pixelFormat: pf];
	mapOrigin.x = 4.0;
	mapOrigin.y = 4.0;
	mapOrigin.z = 0.0;
	mapTexture = 0;
	Xres = 1.0;
	Yres = 1.0;
	calibrated = NO;
	camView = 0;
	bot = robot;
	botPos = [bot roverPosition];
	[mapCalibrationPanel setReleasedWhenClosed:NO];
	/*NSAttributedString *coloredString;
	coloredString = [[NSAttributedString alloc] initWithString:@"Grid Lines" attributes:[NSDictionary dictionaryWithObject:[NSColor whiteColor] forKey:NSForegroundColorAttributeName] ];
	[gridButton setAttributedTitle:coloredString];
	[coloredString release];
	coloredString = [[NSAttributedString alloc] initWithString:@"Map" attributes:[NSDictionary dictionaryWithObject:[NSColor whiteColor] forKey:NSForegroundColorAttributeName]];
	[mapButton setAttributedTitle:coloredString];
	[coloredString release];
	coloredString = [[NSAttributedString alloc] initWithString:@"Waypoints" attributes:[NSDictionary dictionaryWithObject:[NSColor whiteColor] forKey:NSForegroundColorAttributeName]];
	[waypointButton setAttributedTitle:coloredString];
	[coloredString release];
	coloredString = [[NSAttributedString alloc] initWithString:@"WP #'s" attributes:[NSDictionary dictionaryWithObject:[NSColor whiteColor] forKey:NSForegroundColorAttributeName]];
	[waypointNumButton setAttributedTitle:coloredString];
	[coloredString release];*/
    return self;
}

- (void) prepareOpenGL
{
	GLint swapInt = 1;
	//set to vbl sync
	[[self openGLContext] setValues:&swapInt forParameter:NSOpenGLCPSwapInterval];
	//init GL stuff here
	glClearDepth(1.0f);
	glDepthFunc(GL_LESS);
	//glEnable(GL_DEPTH_TEST);
	glShadeModel(GL_SMOOTH);
	glClearColor(0,0,0,0);
	glEnable(GL_TEXTURE_2D);
	//glEnable(GL_COLOR_MATERIAL);
	[self initBotTexture];
	[self initCamera];
	[self buildFont];
	[cursorPosition setStringValue:[NSString stringWithFormat:@"Crosshair (%0.3f,%0.3f)",camera.viewPosition.x,camera.viewPosition.y]];

	timer = [NSTimer scheduledTimerWithTimeInterval:(1.0f/30.0f) target:self selector:@selector(drawFrame) userInfo:nil repeats:YES];
	[[NSRunLoop currentRunLoop] addTimer:timer forMode:NSDefaultRunLoopMode];
	[[NSRunLoop currentRunLoop] addTimer:timer forMode:NSEventTrackingRunLoopMode];
}

- (void) reshape
{
	NSRect rectView = [self bounds];
	camera.viewHeight = rectView.size.height;
	camera.viewWidth = rectView.size.width;
	glViewport (0, 0, camera.viewWidth, camera.viewHeight);
	[self updateProjection];
}

- (void)updateProjection
{
	GLdouble ratio;
	[[self openGLContext] makeCurrentContext];
	
	// set projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	ratio = camera.viewWidth / (float) camera.viewHeight;
	gluPerspective(45.0, (GLfloat)ratio, NEARCLIPPING, FARCLIPPING);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

- (void)initCamera
{
	camera.aperture = 40;
	camera.rotateCenter = mapOrigin;
	
	camera.viewPosition.x = 0.0;
	camera.viewPosition.y = 0.0;
	camera.viewPosition.z = 160.0;
	camera.viewDirection.x = 0.0; 
	camera.viewDirection.y = 0.0; 
	camera.viewDirection.z = 0.0;
	
	camera.viewUp.x = 0;  
	camera.viewUp.y = 1; 
	camera.viewUp.z = 0;
	
	gridStep = (int) camera.viewPosition.z/4;
	[mapScale setIntValue:gridStep];
	botWidth = camera.viewPosition.z*tan(DEGTORAD(2.6));
	botHeight = camera.viewPosition.z*tan(DEGTORAD(1.3));
}

- (void)updateCamera
{
	if(camera.viewPosition.z < 1.0) camera.viewPosition.z = 1.0;
	else if(camera.viewPosition.z > FARCLIPPING) camera.viewPosition.z = FARCLIPPING-2.0;
	if(camView == 0){
	gluLookAt(camera.viewPosition.x
			  , camera.viewPosition.y
			  , camera.viewPosition.z
			  , camera.viewPosition.x
			  , camera.viewPosition.y
			  , camera.viewDirection.z
			  , camera.viewUp.x, camera.viewUp.y, camera.viewUp.z);
	}
	else if(camView == 1){
		gluLookAt(botPos->x, botPos->y, camera.viewPosition.z,
				  botPos->x, botPos->y, camera.viewDirection.z,
				  [bot roverHeading]->x, [bot roverHeading]->y, [bot roverHeading]->z);
		[cursorPosition setStringValue:[NSString stringWithFormat:@"Rover (%0.3f,%0.3f)",botPos->x,botPos->y]];
	}
	else{
		gluLookAt(botPos->x, botPos->y, camera.viewPosition.z,
				  botPos->x, botPos->y, camera.viewDirection.z,
				  camera.viewUp.x,camera.viewUp.y,camera.viewUp.z);
		[cursorPosition setStringValue:[NSString stringWithFormat:@"Rover (%0.3f,%0.3f)",botPos->x,botPos->y]];
	}
}

- (void)overlayFrame
{
	glPushMatrix();
	glLoadIdentity();
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0, [self frame].size.width, 0, [self frame].size.height);
	glColor3f(1, 1, 1);
	glBegin(GL_LINES);
	glVertex2i([self frame].size.width/2-5,[self frame].size.height/2);
	glVertex2i([self frame].size.width/2+5,[self frame].size.height/2);
	glVertex2i([self frame].size.width/2,[self frame].size.height/2+5);
	glVertex2i([self frame].size.width/2,[self frame].size.height/2-5);
	glEnd();
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}

- (void)drawFrame
{
	[self drawRect:[self frame]];
}
- (void)drawRect:(NSRect)bounds
{
	int i;
	static float	gridSize = 2000.0;
	Point3D		gpsPoint;
	glLoadIdentity();
	glClearColor(0,0,0,0);
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	[self updateCamera];	// update the camera view
	
	// place the texture or map image
	if([mapButton state] == NSOnState && mapTexture > 0){
		glEnable(GL_TEXTURE_2D);
		glColor3f(1.0, 1.0, 1.0);
		glBindTexture(GL_TEXTURE_2D, mapTexture);
		glBegin(GL_POLYGON);
		glTexCoord2f(1.0f, 0.0f);
		glVertex3f((GLfloat)scaledMapWidth-mapOrigin.x,(GLfloat)scaledMapHeight-mapOrigin.y,-0.5);
		glTexCoord2f(0.0f, 0.0f);
		glVertex3f(-mapOrigin.x,(GLfloat)scaledMapHeight-mapOrigin.y,-0.5);
		glTexCoord2f(0.0f, 1.0f);
		glVertex3f(-mapOrigin.x,-mapOrigin.y,-0.5);
		glTexCoord2f(1.0f, 1.0f);
		glVertex3f((GLfloat)scaledMapWidth-mapOrigin.x,-mapOrigin.y,-0.5);
		glEnd();
		glDisable(GL_TEXTURE_2D);
	}
	
	// draw grid
	if([gridButton state] == NSOnState){
		glColor3f(1.0,1.0,1.0);
		for(i = 0; i <= gridSize; i += gridStep)
		{
			if(i == 0) glLineWidth(3.0);
			else glLineWidth(1.0);
			glBegin(GL_LINES);
			glVertex3f(-gridSize, i, 0.0);					
			glVertex3f(gridSize, i, 0.0);
			glVertex3f(i, -gridSize, 0.0);							
			glVertex3f(i, gridSize, 0.0);
			glEnd();
		}
		for(i = 0; i >= -gridSize; i -= gridStep)
		{
			glBegin(GL_LINES);
			glVertex3f(-gridSize, i, 0.0);					
			glVertex3f(gridSize, i, 0.0);
			glVertex3f(i, -gridSize, 0.0);							
			glVertex3f(i, gridSize, 0.0);
			glEnd();
		}
	}
	
	// draw Rover
	if(bot){
		glPushMatrix();
		glTranslatef(botPos->x,botPos->y,botPos->z);
		glRotatef(360 - [bot getRobotProperties]->compass+90, 0.0, 0.0, 1.0);
		glEnable(GL_TEXTURE_2D);
		glColor3f(1.0,1.0,0.0);
		glBindTexture(GL_TEXTURE_2D, botTexture);
		glBegin(GL_POLYGON);
		glTexCoord2f(1.0f,0.0f);
		glVertex3f(botWidth,botHeight,0.0);
		glTexCoord2f(0.0f, 0.0f);
		glVertex3f(-botWidth,botHeight,0.0);
		glTexCoord2f(0.0f, 1.0f);
		glVertex3f(-botWidth,-botHeight,0.0);
		glTexCoord2f(1.0f, 1.0f);
		glVertex3f(botWidth,-botHeight,0.0);
		glEnd();
		glDisable(GL_TEXTURE_2D);
		glPopMatrix();
	}
	
	// draw waypoints
	if([waypointButton state] == NSOnState){
		for(i=0;i<[WPList count];i++){
			Waypoint *WP = [WPList objectAtIndex:i];
			switch ([WP state]){
					case WPStateNew:
					glColor3f(1.0, 1.0, 1.0);
						break;
					case WPStateOld:
					glColor3f(0.0, 0.0, 1.0);
						break;
					case WPStateCurrent:
					glColor3f(0.0, 1.0, 0.0);
						break;
					case WPStateSkipped:
					glColor3f(1.0, 0.0, 0.0);
						break;
				default:
					glColor3f(1.0, 1.0, 1.0);
					break;
			}
			glPointSize(6.0);
			glBegin(GL_POINTS);
			glVertex3d([WP Xcord],[WP Ycord],0.0);
			glEnd();
			
			if([waypointNumButton state] == NSOnState){
				glRasterPos3d([WP Xcord], [WP Ycord], 0.2);
				[self glPrint:@"%d",[WP uuid]]; 
			}
		}
	}

	// draw the gps point
	if(calibrated){
		glPointSize(5.0);
		glColor3f(0.0, 1.0, 0.0);
		glBegin(GL_POINTS);
		gpsPoint = [self LatLonCoordsFrom:homePoint to:[bot getRobotProperties]->GPS];
		glVertex3f(gpsPoint.x,gpsPoint.y,0.0);
		glEnd();
	}
	
	[self overlayFrame];
	
	[[self openGLContext] flushBuffer];
}

//////////////////////////////////////////////////////////////////////////////////////////////
// Responder functions and user input
- (BOOL)acceptsFirstResponder
{
	return YES;
}
- (BOOL)becomeFirstResponder
{
	return YES;
}
- (BOOL)resignFirstResponder
{
	return YES;
}
- (void)mouseDown:(NSEvent*)theEvent
{
	mousePoint = [self convertPoint:[theEvent locationInWindow] fromView:nil];
}
- (void)mouseDragged:(NSEvent*)theEvent
{
	NSPoint pt = [self convertPoint:[theEvent locationInWindow] fromView:nil];
	if([theEvent modifierFlags] & NSControlKeyMask)
	{
		camera.viewPosition.z += camera.viewPosition.z*(mousePoint.y - pt.y)*0.005;
		int mod = (int) camera.viewPosition.z/4;
		if(camera.viewPosition.z < 8.0) gridStep = 1.0;
		else gridStep = mod;
		[mapScale setIntValue:gridStep];
		botWidth = camera.viewPosition.z*tan(DEGTORAD(2.6));
		botHeight = camera.viewPosition.z*tan(DEGTORAD(1.3));
	}
	else if(camView == 0){
		camera.viewPosition.x += (mousePoint.x - pt.x)*camera.viewPosition.z*0.0025;
		camera.viewPosition.y += (mousePoint.y - pt.y)*camera.viewPosition.z*0.0025;
		[cursorPosition setStringValue:[NSString stringWithFormat:@"Crosshair (%0.3f,%0.3f)",camera.viewPosition.x,camera.viewPosition.y]];
	}
	mousePoint = pt;
}
- (void)scrollWheel:(NSEvent*)theEvent
{
	float scrl = [theEvent deltaY];
	if(scrl != 0.0){
		camera.viewPosition.z += camera.viewPosition.z*scrl*0.1;
		if(camera.viewPosition.z > FARCLIPPING) camera.viewPosition.z = FARCLIPPING-2.0;
		int mod = (int) camera.viewPosition.z/8;
		if(camera.viewPosition.z < 8.0) gridStep = 1.0;
		else if(camera.viewPosition.z > FARCLIPPING-2.0) gridStep = 250;
		else gridStep = mod*2;
		[mapScale setIntValue:gridStep];
		botWidth = camera.viewPosition.z*tan(DEGTORAD(2.6));
		botHeight = camera.viewPosition.z*tan(DEGTORAD(1.3));
	}
}
- (void)keyDown:(NSEvent*)event
{
	unichar keyChar = 0;
	
	NSString *keyStroke = [event charactersIgnoringModifiers];
	if([keyStroke length] == 0) return;
	if([keyStroke length] == 1){
		keyChar = [keyStroke characterAtIndex:0];
		switch(keyChar){
			case ' ':
				NSLog(@"stop");
				break;
			case 'v':
				if(camView == 2){
					camView = 0;
					camera.viewPosition = *botPos;
					//camera.viewPosition.x = botPos->x;
					//camera.viewPosition.y = botPos->y;
				}
				else camView++;
				break;
		}
	}
	//[super keyDown:event];
}

/*////////////////////////////////////////////////////////////////////////////////////////////
 * Build our bitmap font and load the map texture
 */
- (void)initMapTexture:(NSBitmapImageRep*)bitmap
{
	glGenTextures(1, &mapTexture);
	glBindTexture(GL_TEXTURE_2D, mapTexture);
	
	scaledMapWidth = mapWidth = [bitmap pixelsWide];
	scaledMapHeight = mapHeight = [bitmap pixelsHigh];
	glPixelStorei(GL_UNPACK_ALIGNMENT,1);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, [bitmap pixelsWide], [bitmap pixelsHigh], 0, GL_RGB, GL_UNSIGNED_BYTE, [bitmap bitmapData]);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
}

- (void)initBotTexture
{
	if(bot)
	{
		glGenTextures(1, &botTexture);
		glBindTexture(GL_TEXTURE_2D, botTexture);
		NSImage *image = [NSImage imageNamed:@"rover"];
		NSBitmapImageRep *bitmap = [[NSBitmapImageRep alloc] initWithData:[image TIFFRepresentation]];
		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, [bitmap pixelsWide], [bitmap pixelsHigh], 0, GL_RGB, GL_UNSIGNED_BYTE, [bitmap bitmapData]);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	}
}

- (void) buildFont
{
	NSFont *font;
	
	// 95 since if we do 96, we get the delete character...
	base = glGenLists( 95 );   // Storage for 95 textures (one per character)
	// font = [ NSFont fontWithName:@"Courier-Bold" size:18 ];
	font = [ NSFont fontWithName:@"Helvetica" size:18 ];
	if( font == nil )
		NSLog( @"font is nil\n" );
	if( ![ font makeGLDisplayListFirst:' ' count:95 base:base ] )
		NSLog( @"Didn't make display list\n" );
}

/*
 * Custom GL "Print" Routine; does this Unicode-style
 */
- (void) glPrint:(NSString *)fmt, ...
{
	NSString *text;
	va_list ap;                        // Pointer To List Of Arguments
	unichar *uniBuffer;
	
	if( fmt == nil || [ fmt length ] == 0 )   // If There's No Text
		return;                                // Do Nothing
	
	va_start( ap, fmt );               // Parses The String For Variables
	text = [ [ [ NSString alloc ] initWithFormat:fmt arguments:ap ] autorelease ];
	va_end( ap );                      // Results Are Stored In Text
	
	glPushAttrib( GL_LIST_BIT );       // Pushes The Display List Bits
	glListBase( base - 32 );           // Sets The Base Character to 32
	uniBuffer = calloc( [ text length ], sizeof( unichar ) );
	[ text getCharacters:uniBuffer ];
	// Draws The Display List Text
	glCallLists( [ text length ], GL_UNSIGNED_SHORT, uniBuffer );
	free( uniBuffer );
	glPopAttrib();                     // Pops The Display List Bits
}

- (void)setWPList:(NSMutableArray *)array
{
	WPList = array;
}

- (Point3D)mapOrigin
{
	return mapOrigin;
}
- (void)setWorldRadius:(double)wr
{
	WorldRadius = wr;
}
- (void)setMapName:(NSString*)name
{
	if(mapName)[mapName release];
	mapName = [[NSString alloc] initWithString:name];
}
- (NSString *)mapName
{
	return mapName;
}
- (double)Xres
{
	return Xres;
}
- (double)Yres
{
	return Yres;
}

- (BOOL)calibrated
{
	return calibrated;
}
- (void)calibrateMap
{
	[indicatorField setStringValue:@"Set First Point"];
	[lonDegreesField setDoubleValue:-97.44695485045216];
	[latDegreesField setDoubleValue:35.20808166028];
	[mapCalibrationPanel setFloatingPanel:YES];
	[mapCalibrationPanel orderFront:self];
}
- (IBAction)setCalibrateButton:(id)sender
{
	if([[indicatorField stringValue] isEqual:@"Set First Point"]){
		[latDegreesField validateEditing];
		calPoint[0].lat.dd = DEGTORAD([latDegreesField doubleValue]);
		[lonDegreesField validateEditing];
		calPoint[0].lon.dd = DEGTORAD([lonDegreesField doubleValue]);
		calPointPixel[0] = camera.viewPosition;
		[indicatorField setStringValue:@"Set Second Point"];
		[latDegreesField setDoubleValue:0.0];
		[lonDegreesField setDoubleValue:0.0];
	}
	else{
		[latDegreesField validateEditing];
		calPoint[1].lat.dd = DEGTORAD([latDegreesField doubleValue]);
		[lonDegreesField validateEditing];
		calPoint[1].lon.dd = DEGTORAD([lonDegreesField doubleValue]);
		calPointPixel[1] = camera.viewPosition;
		[mapCalibrationPanel close];
		Point3D calVec = [self LatLonCoordsFrom:calPoint[0] to:calPoint[1]];
		Xres = calVec.x/(calPointPixel[1].x - calPointPixel[0].x);
		if(Xres < 0.0) Xres = -Xres;
		Yres = calVec.y/(calPointPixel[1].y - calPointPixel[0].y);
		if(Yres < 0.0) Yres = -Yres;
		scaledMapWidth = Xres*mapWidth;
		scaledMapHeight = Yres*mapHeight;
		calibrated=YES;
		
		LatLonCoord startWP = [[WPList objectAtIndex:0] coordinates];
		Point3D orgVect = [self LatLonCoordsFrom:calPoint[0] to:startWP];
		mapOrigin.x = orgVect.x + Xres*calPointPixel[0].x;
		mapOrigin.y = orgVect.y + Yres*calPointPixel[0].y;
	}
}
- (void)calibrationFromFile:(double)xpar and:(double)ypar origin:(Point3D)org
{
	Xres = xpar;
	Yres = ypar;
	scaledMapWidth = Xres*mapWidth;
	scaledMapHeight = Yres*mapHeight;
	calibrated=YES;
	mapOrigin = org;
}

- (Point3D)LatLonCoordsFrom:(LatLonCoord)ll1 to:(LatLonCoord)ll2
{
	Point3D result;
	
	result.x = WorldRadius*(ll2.lon.dd - ll1.lon.dd)*1000;
	result.y = WorldRadius*(ll2.lat.dd - ll1.lat.dd)*1000;
	result.z = 0.0;
	return result;
}
- (Point3D)getCrosshairPosition
{
	return camera.viewPosition;
}
- (void)setHomePoint:(LatLonCoord)h
{
	homePoint = h;
}
- (void)dealloc
{
	[timer invalidate];
	[timer release];
	[mapName release];
	[super dealloc];
}
@end
